using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuUtilities
{
    /// <summary>
    /// 包含帮助器数学方法。
    /// </summary>
    public static class MathHelper
    {
        /// <summary>
        /// PI的近似值。
        /// </summary>
        public const float Pi = 3.141592653589793239f;

        /// <summary>
        /// PI的近似值乘以2。
        /// </summary>
        public const float TwoPi = 6.283185307179586477f;

        /// <summary>
        /// PI除以2的近似值。
        /// </summary>
        public const float PiOver2 = 1.570796326794896619f;

        /// <summary>
        /// PI除以4的近似值。
        /// </summary>
        public const float PiOver4 = 0.785398163397448310f;

        /// <summary>
        /// 将值夹在最小值和最大值之间。
        /// </summary>
        /// <param name="value">要钳制的值。</param>
        /// <param name="min">最小值。如果该值小于此值,则改为返回最小值。</param>
        /// <param name="max">最大值。如果该值大于此值,则改为返回最大值。</param>
        /// <return>钳制的值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Clamp(float value, float min, float max)
        {
            if (value < min)
                return min;
            else if (value > max)
                return max;
            return value;
        }


        /// <summary>
        /// 返回两个参数中较高的值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较高值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Max(float a, float b)
        {
            return a > b ? a : b;
        }

        /// <summary>
        /// 返回两个参数的较低值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较低值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Min(float a, float b)
        {
            return a < b ? a : b;
        }

        /// <summary>
        /// 将值夹在最小值和最大值之间。
        /// </summary>
        /// <param name="value">要钳制的值。</param>
        /// <param name="min">最小值。如果该值小于此值,则改为返回最小值。</param>
        /// <param name="max">最大值。如果该值大于此值,则改为返回最大值。</param>
        /// <return>钳制的值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int Clamp(int value, int min, int max)
        {
            if (value < min)
                return min;
            else if (value > max)
                return max;
            return value;
        }


        /// <summary>
        /// 返回两个参数中较高的值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较高值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int Max(int a, int b)
        {
            return a > b ? a : b;
        }

        /// <summary>
        /// 返回两个参数的较低值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较低值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int Min(int a, int b)
        {
            return a < b ? a : b;
        }

        /// <summary>
        /// 将值夹在最小值和最大值之间。
        /// </summary>
        /// <param name="value">要钳制的值。</param>
        /// <param name="min">最小值。如果该值小于此值,则改为返回最小值。</param>
        /// <param name="max">最大值。如果该值大于此值,则改为返回最大值。</param>
        /// <return>钳制的值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static long Clamp(long value, long min, long max)
        {
            if (value < min)
                return min;
            else if (value > max)
                return max;
            return value;
        }


        /// <summary>
        /// 返回两个参数中较高的值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较高值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static long Max(long a, long b)
        {
            return a > b ? a : b;
        }

        /// <summary>
        /// 返回两个参数的较低值。
        /// </summary>
        /// <param name="a">第一个值。</param>
        /// <param name="b">第二个值。</param>
        /// <return>两个参数的较低值。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static long Min(long a, long b)
        {
            return a < b ? a : b;
        }

        /// <summary>
        /// 将度转换为弧度。
        /// </summary>
        /// <param name="度">要转换的度。</param>
        /// <return>等于输入度的弧度。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float ToRadians(float degrees)
        {
            return degrees * (Pi / 180f);
        }

        /// <summary>
        /// 将弧度转换为度。
        /// </summary>
        /// <param name="RADIANS">要转换的弧度。</param>
        /// <return>等于输入弧度的度数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float ToDegrees(float radians)
        {
            return radians * (180f / Pi);
        }


        /// <summary>
        /// 如果值为负值,则返回-1,否则返回1。
        /// </summary>
        /// <param name="x">要计算符号的值。</param>
        /// <return>如果输入为负,则为-1,否则为1。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float BinarySign(float x)
        {
            return x < 0 ? -1 : 1;
        }

        // 请注意,这些cos/sin实现不是为了提高性能,而是为了：
        // 1)提供宽处理的SIMD加速版,以及
        // 2)为需要匹配其行为的系统提供与SIMD版本一致的标量实现。
        // 主要的激励用例是姿势积分器(标量)和扫描测试(广泛向量化)。

        /// <summary>
        /// 计算余弦的近似值。最大误差略高于3E-6。
        /// </summary>
        /// <param name="x">要取余弦的值。</param>
        /// <return>输入值的近似余弦。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Cos(float x)
        {
            // 这主要是为了保持PoseIntegrator和扫描之间的一致性,而不一定是为了相对于Math.Cos的原始性能。
            if (x < 0)
                x = -x;
            var intervalIndex = x * (1f / TwoPi);
            x -= (int)intervalIndex * TwoPi;

            // [0,pi/2]=f(X)
            // (Pi/2,pi]=-f(Pi-x)
            // (Pi,3*pi/2]=-f(x-Pi)
            // (3*pi/2,2*pi]=f(2*pi-x)
            // 这可以做得更巧妙。
            bool negate;
            if (x < Pi)
            {
                if (x < PiOver2)
                {
                    negate = false;
                }
                else
                {
                    x = Pi - x;
                    negate = true;
                }
            }
            else
            {
                if (x < 3 * PiOver2)
                {
                    x = x - Pi;
                    negate = true;
                }
                else
                {
                    x = TwoPi - x;
                    negate = false;
                }
            }

            // 该表达式是从0到Pi/2的有理插值,最大误差略大于3e-6。
            var x2 = x * x;
            var x3 = x2 * x;
            // TODO：如果可用,可以将其重新组织成两个FMA流。
            var numerator = 1 - 0.24f * x - 0.4266f * x2 + 0.110838f * x3;
            var denominator = 1 - 0.240082f * x + 0.0741637f * x2 - 0.0118786f * x3;
            var result = numerator / denominator;
            return negate ? -result : result;

        }
        /// <summary>
        /// 计算正弦的近似值。最大误差略高于3E-6。
        /// </summary>
        /// <param name="x">要取其正弦的值。</param>
        /// <return>输入值的近似正弦。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Sin(float x)
        {
            return Cos(x - PiOver2);
        }

        /// <summary>
        /// 计算余弦的近似值。最大误差略高于3E-6。
        /// </summary>
        /// <param name="x">要取余弦的值。</param>
        /// <return>输入值的近似余弦。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Cos(in Vector<float> x, out Vector<float> result)
        {
            // 这主要是为了保持PoseIntegrator和扫描之间的一致性,而不一定是为了相对于Math.Cos的原始性能。
            var periodX = Vector.Abs(x);
            // TODO：没有可用的发言权或截断.可能会想晚些时候再来看看。
            periodX = periodX - TwoPi * Vector.ConvertToSingle(Vector.ConvertToInt32(periodX * (1f / TwoPi)));

            // [0,pi/2]=f(X)
            // (Pi/2,pi]=-f(Pi-x)
            // (Pi,3*pi/2]=-f(x-Pi)
            // (3*pi/2,2*pi]=f(2*pi-x)
            // 这可以做得更巧妙。
            Vector<float> y;
            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(PiOver2)), new Vector<float>(Pi) - periodX, periodX);
            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(Pi)), new Vector<float>(-Pi) + periodX, y);
            y = Vector.ConditionalSelect(Vector.GreaterThan(periodX, new Vector<float>(3 * PiOver2)), new Vector<float>(TwoPi) - periodX, y);

            // 该表达式是从0到Pi/2的有理插值,最大误差略大于3e-6。
            var y2 = y * y;
            var y3 = y2 * y;
            // TODO：如果可用,可以将其重新组织成两个FMA流。
            var numerator = Vector<float>.One - 0.24f * y - 0.4266f * y2 + 0.110838f * y3;
            var denominator = Vector<float>.One - 0.240082f * y + 0.0741637f * y2 - 0.0118786f * y3;
            result = numerator / denominator;
            result = Vector.ConditionalSelect(
                Vector.BitwiseAnd(
                    Vector.GreaterThan(periodX, new Vector<float>(PiOver2)),
                    Vector.LessThan(periodX, new Vector<float>(3 * PiOver2))), -result, result);
        }
        /// <summary>
        /// 计算正弦的近似值。最大误差略高于3E-6。
        /// </summary>
        /// <param name="x">要取其正弦的值。</param>
        /// <return>输入值的近似正弦。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Sin(in Vector<float> x, out Vector<float> result)
        {
            Cos(x - new Vector<float>(PiOver2), out result);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApproximateAcos(in Vector<float> x, out Vector<float> acos)
        {
            // TODO：可能会比这个做得更好。绝对不需要用平方根。
            // acos(X)~=(pi/(2*sqrt(2)*sqrt(2-2*x),对于0≤x<=1
            // acos(X)~=pi-(pi/(2*sqrt(2)*sqrt(2+2*x),对于-1<=x<=0
            var two = new Vector<float>(2f);
            acos = new Vector<float>(1.11072073454f) * Vector.SquareRoot(Vector.Max(Vector<float>.Zero, two - two * Vector.Abs(x)));
            acos = Vector.ConditionalSelect(Vector.LessThan(x, Vector<float>.Zero), new Vector<float>(Pi) - acos, acos);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Floor(in Vector<float> x, out Vector<float> result)
        {
            // 这远远不是理想的。您可能会做得更好-特别是使用平台内部特性。
            var intX = Vector.ConvertToInt32(x);
            result = Vector.ConvertToSingle(Vector.ConditionalSelect(Vector.LessThan(x, Vector<float>.Zero), intX - Vector<int>.One, intX));
        }

        /// <summary>
        /// 获取从a到b的角度变化,作为从-pi到pi的有符号值。
        /// </summary>
        /// <param name="a">源角度。</param>
        /// <param name="b">目标角度。</param>
        /// <param name="Difference">a和b之间的差异,表示为-pi到pi之间的值。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetSignedAngleDifference(in Vector<float> a, in Vector<float> b, out Vector<float> difference)
        {
            var pi = new Vector<float>(Pi);
            var half = new Vector<float>(0.5f);
            var x = (b - a) * new Vector<float>(1f / TwoPi) + half;
            Floor(x, out var flooredX);
            difference = (x - flooredX - half) * TwoPi;
        }
    }
}
